#ifndef __GSL_UTILS_NURBS__
#define __GSL_UTILS_NURBS__

#include <vector>
#include <list>
#include <iostream>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/tuple/tuple.hpp>

#include <gsl/gsl_fit.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>

#include "../math/vector.h"

namespace GSL_wrapper {

template<class type>
std::pair<type, type> LinearFit(const std::vector<type>& x, const std::vector<type>& y) {
	int i;
	const int n = x.size();

	type c0, c1, cov00, cov01, cov11, chisq;

	gsl_fit_linear (&(x[0]), 1, &(y[0]), 1, n, 
			&c0, &c1, &cov00, &cov01, &cov11, 
			&chisq);

// 	printf ("# best fit: Y = %g + %g X\n", c0, c1);
// 	printf ("# covariance matrix:\n");
// 	printf ("# [ %g, %g\n#   %g, %g]\n", 
// 			cov00, cov01, cov01, cov11);
// 	printf ("# chisq = %g\n", chisq);

	return std::pair<type, type>(c0, c1);
}

template<class type>
std::pair<type, type> LinearFit(const std::list< math::Vec<type, 2> >& sample) {
	std::vector<type> x, y;
	for(typename std::list< math::Vec<type, 2> >::const_iterator it = sample.begin(); it != sample.end(); ++it) {
		const math::Vec<type, 2>& p = *it;
		x.push_back(p[0]);
		y.push_back(p[1]);
	}
	return LinearFit(x, y);
}

template<class type>
std::pair<type, type> LinearFit(const std::vector< std::pair<type, type> >& sample) {
	std::vector<type> x, y;
	for(typename std::vector< std::pair<type, type> >::const_iterator it = sample.begin(); it != sample.end(); ++it) {
		const std::pair<type, type>& p = *it;
		x.push_back(p.first);
		y.push_back(p.second);
	}
	return LinearFit(x, y);
}

template<class T>
std::vector< std::pair<boost::numeric::ublas::vector<double, std::vector<double> >, double> > Eigensystem(const T& A) {
	double* data = new double[A.size1() * A.size2()];
	for(size_t i = 0; i < A.size1(); i++) {
		for(size_t j = 0; j < A.size2(); j++) {
			data[i * A.size1() + j] = A(i, j);
		}
	}
	gsl_matrix_view m = gsl_matrix_view_array(data, A.size1(), A.size2());
	gsl_vector* eval = gsl_vector_alloc(A.size1());
	gsl_matrix* evec = gsl_matrix_alloc(A.size1(), A.size2());
	gsl_eigen_symmv_workspace* w = gsl_eigen_symmv_alloc(A.size1());

	gsl_eigen_symmv(&m.matrix, eval, evec, w);
	gsl_eigen_symmv_free(w);
	gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);

	std::vector< std::pair<boost::numeric::ublas::vector<double, std::vector<double> >, double> > r(A.size1());
	for(size_t i = 0; i < A.size1(); i++) {
		r[i].first = boost::numeric::ublas::vector<double, std::vector<double> >(A.size1());
		for(size_t j = 0; j < A.size1(); j++)
			r[i].first(j) = gsl_matrix_get(evec, j, i);

		r[i].second = gsl_vector_get(eval, i);
	}

	gsl_vector_free(eval);
	gsl_matrix_free(evec);

	delete [] data;

	return r;
}

template<class T>
T Max(const boost::numeric::ublas::matrix<T>& A) {
	T max = (T)0.0;
	for(size_t i = 0; i < A.size1(); i++) {
		for(size_t j = 0; j < A.size2(); j++) {
			const T aij = std::abs(A(i, j));
			if(max < aij)
				max = aij;
		}
	}
	return max;
}

template<class T>
boost::numeric::ublas::matrix<T> ExtractRotation(const boost::numeric::ublas::matrix<T>& A) {
	typedef boost::numeric::ublas::matrix<T> Matrix;
	typedef boost::numeric::ublas::zero_matrix<T> ZeroMatrix;

	const size_t n = A.size1();
	double* data = new double[n * n];
	for(size_t i = 0; i < n; i++) {
		for(size_t j = 0; j < n; j++) {
			data[i * n + j] = A(i, j);
		}
	}
	
	gsl_matrix_view a = gsl_matrix_view_array(data, n, n);
	gsl_matrix* v = gsl_matrix_alloc(n, n);
	gsl_vector* s = gsl_vector_alloc(n);
	gsl_vector* work = gsl_vector_alloc(n);
	
	gsl_matrix* u = gsl_matrix_alloc(n, n);
	gsl_matrix_memcpy(u, &a.matrix);
	gsl_linalg_SV_decomp(u, v, s, work);

	Matrix U(n, n);
	Matrix V(n, n);
	Matrix S = ZeroMatrix(n, n);
	for(size_t i = 0; i < n; i++) {
		S(i, i) = gsl_vector_get(s, i);
		for(size_t j = 0; j < n; j++) {
			U(i, j) = gsl_matrix_get(u, i, j);
			V(i, j) = gsl_matrix_get(v, i, j);
		}
	}
	
	gsl_vector_free(s);
	gsl_vector_free(work);
	gsl_matrix_free(v);
	gsl_matrix_free(u);
	delete [] data;

	Matrix US = prod(U, S);
	Matrix USVT = prod(US, trans(V));
	Matrix Diff = USVT - A;
	if(Max<T>(Diff) > 0.0001) {
		std::cerr << "error: problem with SVD." << std::endl;
	}

	Matrix R = prod(U, trans(V));

	return R;
}

template<class T>
boost::tuples::tuple<boost::numeric::ublas::matrix<T>, boost::numeric::ublas::matrix<T>, boost::numeric::ublas::matrix<T> > SVD(const boost::numeric::ublas::matrix<T>& A) {
	typedef boost::numeric::ublas::matrix<T> Matrix;
	typedef boost::numeric::ublas::zero_matrix<T> ZeroMatrix;

	const size_t n = A.size1();
	double* data = new double[n * n];
	for(size_t i = 0; i < n; i++) {
		for(size_t j = 0; j < n; j++) {
			data[i * n + j] = A(i, j);
		}
	}
	
	gsl_matrix_view a = gsl_matrix_view_array(data, n, n);
	gsl_matrix* v = gsl_matrix_alloc(n, n);
	gsl_vector* s = gsl_vector_alloc(n);
	gsl_vector* work = gsl_vector_alloc(n);
	
	gsl_matrix* u = gsl_matrix_alloc(n, n);
	gsl_matrix_memcpy(u, &a.matrix);
	gsl_linalg_SV_decomp(u, v, s, work);

	Matrix U(n, n);
	Matrix V(n, n);
	Matrix S = ZeroMatrix(n, n);
	for(size_t i = 0; i < n; i++) {
		S(i, i) = gsl_vector_get(s, i);
		for(size_t j = 0; j < n; j++) {
			U(i, j) = gsl_matrix_get(u, i, j);
			V(i, j) = gsl_matrix_get(v, i, j);
		}
	}
	
	gsl_vector_free(s);
	gsl_vector_free(work);
	gsl_matrix_free(v);
	gsl_matrix_free(u);
	delete [] data;

	return boost::tuples::tuple<boost::numeric::ublas::matrix<T>, boost::numeric::ublas::matrix<T>, boost::numeric::ublas::matrix<T> >(U, V, S);
}

}

#endif //__GSL_UTILS_NURBS__
